clear all;
close all;
imaqreset();

tic

%get video inputs 
cvi = videoinput('mwkinectimaq', 1,'RGB_1280x960');
% cvi = videoinput('mwkinectimaq', 1);
img_high=getsnapshot(cvi);

%vid_1 = videoinput('kinect', 1,'RGB_1280x960');
cvi = videoinput('mwkinectimaq', 1);
dvi = videoinput('mwkinectimaq', 2);
%Set manual triggering and frame count
    triggerconfig([cvi dvi], 'manual');
    set([cvi dvi], 'FramesPerTrigger', 1);
    %Start input devices
    start([cvi dvi]);
    %Trigger input devices
    trigger([cvi dvi]);
    
    %Get data from devices
    c = rgb2gray(getdata(cvi));
    d = bitshift(getdata(dvi), 3);
      
    %Stop input devices
    stop([cvi dvi]);
    %%%%%The depth map is aligned with the RGB figure by using the function
    %%%%%depth2colourmap 
    %Calculate map
    map = depth2colormap(d');
    %Generate mapped color image
    cMapped = uint8(zeros(size(c)));
    for i = 1:size(map, 1)
        for j = 1:size(map,2)
            x = map(i,j,2);
            y = map(i,j,1);

            if (x > 0) && (y > 0) && ...
                (x < size(c, 1)) && (y < size(c, 2))
                cMapped(i, j) = c(x, y);
            end
        end
    end
img_1 = cMapped;
% img_1_resize = imresize(img_1, 2);  %from 480*640 to 960*1280 for calibration, cali done in 960*1280 image
img_2 = getsnapshot(dvi);


%%%%%Change the contrast of the depth map
% limit = [750, 1600];
% colormap(bone(128));
% imagesc(img_2, limit)
% f=getframe;
% image=f.cdata;
% image_gray=rgb2gray(image);


img_gray_1 = img_1;
img_high_gray=rgb2gray(img_high);

toc

%% load calibration data 
loading_calib 
%% create calibrated image
% [y_pix_size,x_pix_size]=size(img_gray_1);
% calibrate_img_1=zeros(size(img_gray_1));

%normalize([y_pix_size;x_pix_size],fc,cc,kc,alpha_c);


%%%%%% Atempt at Sift detection of cups, however not used in the final code, left here for development purposes

tic
 cup_1 = rgb2gray(imread('cup_test.JPG'));
 
 [f_cup_1 d_cup_1] = vl_sift(single(cup_1));
 [f_img_gray_1 d_img_ray_g1] = vl_sift(single(img_high_gray));
 
 [matches scores] = vl_ubcmatch(d_cup_1, d_img_ray_g1);
 
 [scores_sorted, indices] = sort(scores, 'descend');
 
 No_feat=23;
 matches = matches(:, indices(1:No_feat));
 figure(6);
 [input_points base_points] = visualise_sift_matches(cup_1, img_high_gray, f_cup_1, f_img_gray_1, matches);

 avg_y_1=round(median(base_points(:,2))/2);
 avg_x_1=round(median(base_points(:,1))/2);
 %%%%%%%%%%%%%%%%%%%%%%%%%%
 [f_cup_1 d_cup_1] = vl_sift(single(cup_1));
 [f_img_gray_1 d_img_ray_g1] = vl_sift(single(img_high_gray));
 
 [matches scores] = vl_ubcmatch(d_cup_1, d_img_ray_g1);
 
 [scores_sorted, indices] = sort(scores, 'descend');
 
 No_feat=23;
 matches = matches(:, indices(1:No_feat));
 figure(6);
 [input_points base_points] = visualise_sift_matches(cup_1, img_high_gray, f_cup_1, f_img_gray_1, matches);

 avg_y_2=round(median(base_points(:,2))/2);
 avg_x_2=round(median(base_points(:,1))/2); 
 %%%%%%%%%%%%%%%%%%%%%%%%%%
 [f_cup_1 d_cup_1] = vl_sift(single(cup_1));
 [f_img_gray_1 d_img_ray_g1] = vl_sift(single(img_high_gray));
 
 [matches scores] = vl_ubcmatch(d_cup_1, d_img_ray_g1);
 
 [scores_sorted, indices] = sort(scores, 'descend');
 
 No_feat=23;
 matches = matches(:, indices(1:No_feat));
 figure(6);
 [input_points base_points] = visualise_sift_matches(cup_1, img_high_gray, f_cup_1, f_img_gray_1, matches);

 avg_y_3=round(median(base_points(:,2))/2);
 avg_x_3=round(median(base_points(:,1))/2);  
 
 avg_x=round(mean(avg_x_1:avg_x_2:avg_x_3));
 avg_y=round(mean(avg_y_1:avg_y_2:avg_y_3));
 
 toc
% display(dvi(round(avg_y),round(avg_x)));
% figure(2);
% imshow(img_1((round(avg_y)-480/10):(round(avg_y)+480/10),(round(avg_x)-640/10):(round(avg_x)+640/10)));


%% origin finding
%[rot, trans]  = pose;


%%%Blur the RGB image to reduce back ground noise
% hsize =400;
% sigma= 10;

tic 
hsize =100;
sigma= 1; 

K = fspecial('gaussian', hsize, sigma);
Igf = imfilter(img_gray_1, K);
imshow(Igf);
%%%%The edge detection of the cups to find the elipse at the top using
%%%%'canny' method
thresh=0.4;
sigma=5;
E = edge(Igf, 'canny',thresh,sigma);
figure(4);
imshow(E);
toc
% determine the number of objects in the image
Index_med=1;
Index_large=1;
Index_small=1;
%%%define size of cups
centre_small_size(1)=0;
centre_med_size(1)=0;
centre_large_size(1)=0;


%%%Sorting cups according to size 
tic 

[imgN, num] = bwlabel(E,8);
% 
 f = regionprops(E,'BoundingBox','Centroid','Extent','Area','MajorAxisLength','MinorAxisLength');
      for i=1:num
         area=f(i).Area;
         major=f(i).MajorAxisLength;
         minor=f(i).MinorAxisLength;
         % blob_area=major/2*minor/2*pi;
        %%%% Compare majot and minor axis to determine the area of enclosed
        %%%% objects
        error=(major-minor);
         if (abs(error)<16) 
             pix_loc=f(i).Centroid;
             depth=double(img_2(round(pix_loc(2)),round(pix_loc(1))));
             %distance=(depth+21)/10.357;
             distance=35;  
             %%%% Formula for cup size area vs deapth readings
             
             area_med=0.0431*distance^2 - 11.243*distance + 846.66;   
             area_large=0.0542*distance^2 - 14.018*distance + 1030.6;
             area_small=0.0335*distance^2 - 8.361*distance + 622.18;
           
             %% camera placed 50 cm from the ground
             area_med=454;   
             area_large=525;
             area_small=324;          
             
             
             %display(distance);
             %display(area_med);
%              display(area_small);
%               display(area_med);
%               display(area_large);
          precison=50;
          scale=1;
        %%% The detection for cup sizes which inovlves using the depth of
        %%% the detected cup and then plugiing into the formula for the
        %%% disnatce vs area to find the cup size
        %% normalize convert the pixel space to 3D space in camera reference frame
         if(area*scale>(area_small*scale-precison-10) && area*scale<(area_small*scale+precison+3))
             display('small cup');
            % display(depth);
            % display(distance);
              centre_small_size(Index_small)=f(i).Area;
              centre_small(Index_small,:)=f(i).Centroid;
              
             x=centre_small(Index_small,1);
             y=centre_small(Index_small,2);
             x_pixel=[x;y];
             xn = normalize(x_pixel,fc,cc,kc,alpha_c);
             distance=38;
             x_cam=xn*distance;
             x_cam(1)=x_cam(1)*-1;
             x_cam(2)=x_cam(2)*-1;
             small_actual(Index_small,:)=[x_cam(1) x_cam(2)  distance];
              
              
              Index_small=Index_small+1; 
              
         elseif(area*scale>(area_med*scale-precison) && area*scale<(area_med*scale+precison))
              % filter(Index,1:4)=f(i).BoundingBox;
              display('medium cup');
           %  display(depth);
            % display(distance);
              centre_med(Index_med,:)=f(i).Centroid;
               centre_med_size(Index_med)=f(i).Area;
               
             x=centre_med(Index_med,1);
             y=centre_med(Index_med,2);
             x_pixel=[x;y];
             xn = normalize(x_pixel,fc,cc,kc,alpha_c);
             x_cam=xn*distance;
             x_cam(1)=x_cam(1)*-1;
             x_cam(2)=x_cam(2)*-1;
             distance=35;
             med_actual(Index_med,:)=[x_cam(1) x_cam(2)  distance];  
               
               
              Index_med=Index_med+1;     
              
         elseif (area*scale>(area_large*scale-precison) && area*scale<(area_large*scale+precison+40))
             display('large cup');
          %   display(depth);
          %   display(distance);
             centre_large(Index_large,:)=f(i).Centroid;
             centre_large_size(Index_large)=f(i).Area;
             
             x=centre_large(Index_large,1);
             y=centre_large(Index_large,2);
             x_pixel=[x;y];
             xn = normalize(x_pixel,fc,cc,kc,alpha_c);
             distance=33;
             x_cam=xn*distance;
             x_cam(1)=x_cam(1)*-1;
             x_cam(2)=x_cam(2)*-1;
             large_actual(Index_large,:)=[x_cam(1) x_cam(2)  distance];
             
              Index_large=Index_large+1;  
              
%          elseif(area>120 && area<=600) 
%              origin_pic(1,:)=f(i).Centroid;
            
         end   
         end
      end
toc
%     
% 
% imshow(E(1:640,1:1240))
% hold on
% 
% Index=Index-1;
% I=1;
% while I<=Index
% % 
% a= filter(I,1:4); 
% location(I,1:2)=[a(1)+a(3)/2;a(2)+a(4)/2]
% % x_cor=a(1);
% % y_cor=a(2);
% % x_width=a(3);
% % y_width=a(4);
% % 
% % plot(a(2):(a(2)+y_width),x_cor, 'b*');
% % plot(a(2),x_cor:(x_cor+a(3)), 'b*');
% % hold on
%  I=I+1;
%  end

%   s  = regionprops(E(150:450,250:400), 'centroid','Area');
%    centroids = cat(1, s.Centroid);
%    imshow(E(150:450,250:400))
%    hold on
%    plot(centroids(:,1), centroids(:,2), 'b*')
%    hold off

% Index_med=Index_med-1;
%  for i=1:Index_med
%  figure;
%  imshow(E(1:round(centre_med(i,2)+10),1:round(centre_med(i,1)+10)));
%  hold on;
%  end
%  
% Index_large=Index_large-1;
%  for i=1:Index_large
%  figure;
%  imshow(E(1:round(centre_large(i,2)+10),1:round(centre_large(i,1)+10)));
%  hold on;
%  end
%  
% Index_small=Index_small-1;
%  for i=1:Index_small
%  figure;
%  imshow(E(1:round(centre_small(i,2)+10),1:round(centre_small(i,1)+10)));
%  hold on;
% end
%%%%Plot the center of image with differnt colours depeding on the size of
%%%%the cup where red is medium, blue is large and grenn is small
tic 
figure(3);
imshow(img_1);
hold on;
% plot(round(origin_pic(1,2)),round(origin_pic(1,1)),'y*');
if centre_large_size(1)>0
centroids_large=cat(1,centre_large);
plot(centroids_large(:,1), centroids_large(:,2), 'b*');
end

if centre_med_size(1)>0
centroids_med=cat(1,centre_med);
plot(centroids_med(:,1), centroids_med(:,2), 'r*');
end

if centre_small_size(1)>0
centroids_small=cat(1,centre_small);
plot(centroids_small(:,1), centroids_small(:,2), 'g*');
end 

plot(avg_x,avg_y, 'y*');


if centre_large_size(1)>0
display(large_actual);
%large_actual=rot*transpose(large_actual);   

% large_actual=large_actual*-1;
% large_actual(1,:,:)=large_actual(1,:,:)+13;
% large_actual(2,:,:)=large_actual(2,:,:)-2;
% display(large_actual);    
end
%%%%%Convert camera space to real world reference frame using rotation
%%%%%matrix using SURF and display the 3D camera followed by the real world coordinates

if centre_med_size(1)>0
display(med_actual);  
%med_actual=rot*transpose(med_actual);

% med_actual=med_actual*-1;
% med_actual(1,:,:)=med_actual(1,:,:)+13;
% med_actual(2,:,:)=med_actual(2,:,:)-2;
%display(med_actual);
end

if centre_small_size(1)>0
display(small_actual);
%small_actual=rot*transpose(small_actual);
% small_actual=small_actual*-1;
% small_actual(1,:,:)=small_actual(1,:,:)+13;
% small_actual(2,:,:)=small_actual(2,:,:)-2;
%display(small_actual);
end

toc